#include "../include/car_control/car_control.h"

namespace CAR_CONTROL
{
carControl::carControl() :frist_sn_(false), serial_timeout_(20),faultSymbol_flag(0)
{
  ros::NodeHandle pravite_nh("~");
  //topic_name & frame_id
  pravite_nh.param("debug",     if_debug_,  false);
  pravite_nh.param("car_topic",car_topic_,std::string("/cmd_vel"));
  pravite_nh.param("base_frame", base_frame_id_, std::string("base_footprint")); 
  //serial                                                 
  pravite_nh.param("port", serial_port_, std::string("/dev/carBus")); 
  pravite_nh.param("baud", serial_baud_, 115200);
  //publisher
  //subscribe 接受上层下发的速度指令
  cmd_sub_ = nh_.subscribe<geometry_msgs::Twist>(car_topic_.c_str(),1,&carControl::cmd_vel_callback,this);      

  // 设置速度发布定时器 timer1每50ms触发一次callback1函数
  // ros::Time::init(); 
   
  //setp up serial
  // printf("---------------------------------------------\r\n");
  try
  {
   
    serial_.setPort(serial_port_);
       printf("---------------------------------------------\r\n");
    serial_.setBaudrate(serial_baud_);
    serial_.setFlowcontrol(serial::flowcontrol_none);
            
    serial_.setParity(serial::parity_none); //default is parity_none
    serial_.setStopbits(serial::stopbits_one);
    serial_.setBytesize(serial::eightbits);
    serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
    serial_.setTimeout(time_out);

    serial_.open();

  }
  catch (serial::IOException &e)
  {
    ROS_ERROR_STREAM("Unable to open port ");
    exit(0);
  }
  if (serial_.isOpen())
  {
    ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
    ROS_ERROR_STREAM("Unable to initial Serial port ");
    exit(0);
  }
  ros::Timer send_speed_timer = nh_.createTimer(ros::Duration(0.05),&carControl::send_speed_callback,this); 
  
  
  boost::thread recv_thread(boost::bind(&carControl::receive_func,this));     
  ros::spin(); //开启回调循环
  // processLoop();

}

carControl::~carControl()
{
  boost::mutex::scoped_lock look(mutex_);
  if (serial_.isOpen())
    serial_.close();
}
//辅助函数，计算串口通信校验位
void carControl::check_sum(uint8_t* data, size_t len, uint8_t& dest)
{
    dest = 0x00;
    for(int i=0;i<len;i++)
    {
        dest += *(data + i);
    }
}
int carControl::write_faultSymbolState(faultSymbolState bit)
{
    faultSymbol_flag |= (1<<bit) ;
    return faultSymbol_flag;
}
//健康函数
// void carControl::regularHealthExamination()
// {


// }
//  /cmd_vel速度话题回调函数
void carControl::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_mutex_.unlock();
    }
    catch(...)
    {
        cmd_vel_mutex_.unlock();
    }
}
//对cmd_vel数据进行处理
void carControl::cmd_data_processing(double& x,double& z, uint8_t& mode)
{
  double x_w, z_w;
  if((x!=0)&&(z!=0)){
    mode = error_default;
  }
  if(mode==startMode){
    x_w= 0.08;
    z_w = 0.30;
  }
  else if((mode > startMode)&&(mode <= timingMode)){
    x_w= 0.08;
    z_w = 0.25;
  }
  else{
    x_w= 0;
    z_w = 0;
    x =0;
    z = 0;
  }
    if((x > 0)&&(x <= 1)){
      x = round((1684-1024)*x_w)+1024;  
    }
    else if(x==0){
      x = 1024;  
    }
    else if((x<0)&&(x>=-1)){
      x_w = (-x_w);
      x = round((1024-364)*x_w)+1024;
    }
    else{
       x = 1024;    
    }   
    if((z > 0)&&(z <= 1)){
      z = round((1024-364)*z_w)+1024;
    }
    else if(z==0){
      z = 1024;
    }
    else if((z<0)&&(z>=-1)){
      z_w = -z_w;
      z = round((1684-1024)*z_w)+1024;
    } 
    else{
      z = 1024;  
    }

    
    //将当前标志记录下来，以便下次使用
    if(x !=1024 )   
      x_last = 1; 
    else
      x_last = 0;  
    
    if(z != 1024)
      z_last = 1;  
    else
      z_last = 0;   


}


//启动，停止，运行模式处理
void carControl::Pattern_handler(double& x,double& z)
{
    uint8_t test = error_default;
   
    switch (car_mode)
    {
    case startMode:
      if((x_last==0)&&(z_last ==0)&&((z!=0)||(x!=0)))   //直行模式 转弯模式
      {
        test = car_mode;
        car_mode = timingMode;
        running_count = ros::Time::now();
        ROS_DEBUG_STREAM("CMD_VEL OK!!!!\r\n");
      }
      else if((x!=0)&&(z!=0))
      {
        ROS_DEBUG_STREAM("CMD_VEL ERROR!!!!\r\n");
        test = error_default;
      }
      else
      {
        ROS_DEBUG_STREAM("CMD_VEL call not data!!!!\r\n");
        test = error_default;
      }
      break;
    case stopMode:
      test = error_default;
      if((x!=0)||(z!=0))
      {
        car_mode = startMode;
      }
      ROS_DEBUG_STREAM("stopMode !!!!\r\n");
      break;
    case runningMode:
      break;
    case transitionMode:
      if((x_last==1)&&(z != 0))   //直行变转弯
      {
        ROS_DEBUG_STREAM("直行变转弯 !!!!\r\n");
      }
      else if((z_last==1)&&(x != 0))   //转弯变直行
      {
        ROS_DEBUG_STREAM("转弯变直行 !!!!\r\n");
      }
      else if((x_last==1)&&(x != 0))  //继续直行
      {
        ROS_DEBUG_STREAM("继续直行 !!!!\r\n");
      }
      else if((z_last==1)&&(z != 0))  //继续转弯
      {
        ROS_DEBUG_STREAM("继续转弯 !!!!\r\n");
      }
      else{                           //停止状态
        car_mode = stopMode;
        printf("============================\r\n");
      }
      test = transitionMode;
      ROS_DEBUG_STREAM("transitionMode !!!!\r\n");
        break;
    case timingMode:
      if((x==0)&&(z==0))
      {
        car_mode = stopMode;
      }
      if((ros::Time::now() - running_count).toSec() > 0.1)
      {
        car_mode = transitionMode;
      }

      test = startMode;
      ROS_DEBUG_STREAM("timingMode !!!!\r\n");
      break;
    default:
      test = error_default;
      ROS_DEBUG_STREAM("default !!!!\r\n");
      break;
    }
      cmd_data_processing(x, z, test);
}
void carControl::send_speed_callback(const ros::TimerEvent&)
{
  // printf("++++++++++++++++++++++++++++++++++++++++++++++++++\r\n");
    if((ros::Time::now() - last_twist_time_).toSec() <= 1.0)
    {
        linear_speed = current_twist_.linear.x;
        angular_speed = -current_twist_.angular.z;
        // printf("linear_speed: %lf  angular_speed:  %lf \r\n",linear_speed,angular_speed);
    }
    else
    {
        linear_speed = 0.0;
        angular_speed = 0.0;
    }
    Pattern_handler(linear_speed,angular_speed);
    linear = (int)(linear_speed);
    angular = (int)(angular_speed);
    // printf("linear: %d  angular:  %d \r\n",linear,angular);
    //默认初始化数组为0
    uint8_t data[18];
    data[0] = (0xFF & angular);
    data[1] = ((0xFF00 & angular)>>8);
    data[2] = (0xFF & linear);
    data[3] = ((0xFF00 & linear)>>8);
    data[4] = 5;
    data[6] = 0;
    data[7] = 4;
    data[8] = 0;
    data[9] = 4;
    check_sum(data,17,data[17]);
    serial_.write(data, 18);
    ROS_DEBUG_STREAM("send angular_speed -> left: " << angular_speed <<"; linear_speed: " << linear_speed);

}
//用于接收car的数据，判断通信是否良好
void carControl::receive_func()
{
    uint8_t payload_size, check_num, buffer_data[255],payload_type;
    state_ = waitingForHead;
    size_t type_s;
    bool recv_flag_ = true;
    last_time_ = ros::Time::now();
    while(recv_flag_)
    {
      //记录当前的时间
      now_ = ros::Time::now();
      //判断接收是否超时
      if((now_ - last_time_).toSec() > 1.0)
      {
        //通讯故障标志，请处理
        write_faultSymbolState(receiveTimeout);
        ROS_DEBUG_STREAM("communication failure\r\n");
      }
        switch (state_)
        {
            case waitingForHead:
                check_num = 0x00;       
                type_s = serial_.read(buffer_data, 1);
                state_ = buffer_data[0] == head ? waitingForPayload : waitingForHead;
                if(state_ == waitingForHead)
                {
                    ROS_DEBUG_STREAM("recv head error : ->"<<(int)buffer_data[0]);
                }
                break;
            case waitingForPayload:
                type_s = serial_.read(&buffer_data[1], 19);
                state_ = waitingForCheckSum;
                break;
            case waitingForCheckSum:
                check_sum(buffer_data,19,check_num);
                state_ = buffer_data[19] == check_num ? handlePayload : waitingForHead;
                if(state_ == waitingForHead)
                {
                    ROS_DEBUG_STREAM("check sum error! recv is  : ->"<<(int)buffer_data[19]<<"  calc is "<<check_num);
                }
                break;
            case handlePayload:
                //处理判断结果
                last_time_ = ros::Time::now();
                state_ = waitingForHead;
                break;
        
            default:
                state_ = waitingForHead;
                break;
        }
    }

}
void carControl::processLoop()
{
  ROS_INFO("carControl::processLoop: start");
  while (ros::ok())
  {
    if (!serial_.isOpen())
    {
      ROS_WARN("serial unopen");
    } 

  }

}




} //namespace CAR_CONTROL

int main(int argc, char **argv)
{
  ros::init(argc, argv, "car_control");
  CAR_CONTROL::carControl bp;

  return 0;
}